import com.cyberbotics.webots.controller.DifferentialWheels;
import com.cyberbotics.webots.controller.LightSensor;

public class ProportionalAggressive extends DifferentialWheels {

	private static int S_FRONT_LEFT = 0; // Sensor front left
	private static int S_FRONT_RIGHT = 1; // Sensor front right

	// Time to next step
	private final int TIME_STEP = 100;

	// Light Sensors
	private LightSensor[] _lightSensors = new LightSensor[2];

	// upper row - for left velocity - light values from 0-2
	// lower row - for right velocity - light values from 0-2
	private double[][] sensorMatrix = {
			{ 0.2, 0 },
			{ 0, 0.2 }};

	private double[] sensorValues = new double[2];
	private double[] speedValues = new double[2];


	/**
	 * Constructor
	 */
	public ProportionalAggressive() {
		super();
		initSensors();
	}

	protected void initSensors() {
		_lightSensors = new LightSensor[2];
		// get lightSensor and assign that sensor to the position in the list
		_lightSensors[0] = getLightSensor("ls7");
		_lightSensors[1] = getLightSensor("ls0");
		for (LightSensor ls : _lightSensors) {
			ls.enable(10);
		}
	}

	private void drive(double[] values) {
		setSpeed(values[0], values[1]);
	}

	/**
	 * User defined function for initializing and running the BangBangFollowTheWall class
	 */
	private void align() {
		while (step(TIME_STEP) != -1) {

			sensorValues[0] = _lightSensors[S_FRONT_LEFT].getValue();
			sensorValues[1] = _lightSensors[S_FRONT_RIGHT].getValue();

			speedValues = Matrix.multiply(sensorMatrix, sensorValues);
			drive(speedValues);
		}
	}

	/**
	 * Main method - in this method an instance of the controller is created and the method to launch the robot is called.
	 *
	 * @param args
	 */
	public static void main(String[] args) {
		ProportionalAggressive controller = new ProportionalAggressive();
		// Search the light as first state
		controller.align();
	}

}
